<?php

namespace Helper;


/**
 * edit by 谢宝新 2017-11-05
 */
class Trilateration_xbx
{
    /**
     * @var Circle
     * @access private
     */
    private $circleA;

    /**
     * @var Circle
     * @access private
     */
    private $circleB;

    /**
     * @var Circle
     * @access private
     */
    private $circleC;

    /**
     * @var bool
     * @access private
     */
    private $inMiles;

    /**
     * Trilateration constructor.
     */
    public function __construct($objCircleA,$objCircleB,$objCircleC)
    {
        $this->circleA = new Circle($objCircleA['x'], $objCircleA['y'], $objCircleA['z'],$objCircleA['dist']);
        $this->circleB = new Circle($objCircleB['x'], $objCircleB['y'], $objCircleB['z'],$objCircleB['dist']);
        $this->circleC = new Circle($objCircleC['x'], $objCircleC['y'], $objCircleC['z'],$objCircleC['dist']);
    }

    /**
     * 三角质心
     * get3PointCenter
     * @return array
     * Created by: xiebaoxin
     */
    public function get3PointCenter()
    {
        $triPtX = sprintf("%.2f",($this->circleA->getx()+$this->circleB->getx()+$this->circleC->getx())/3);
        $triPtY = sprintf("%.2f",($this->circleA->gety()+$this->circleB->gety()+$this->circleC->gety())/3);
        $triPtZ = sprintf("%.2f",($this->circleA->getz()+$this->circleB->getz()+$this->circleC->getz())/3);
        return compact('triPtX', 'triPtY','triPtZ');
    }

    /**
     * 3个月两两相交,没有公共区域时候，其中2直线相交的点 z=0
     * getPoint2Circle
     * @return array
     * Created by: xiebaoxin
     */
    public function getPoint2Circle()
    {
        $x1=$this->circleA->getx();
        $y1=$this->circleA->gety();
        $r1=$this->circleA->getRadius();

        $x2=$this->circleB->getx();
        $y2=$this->circleB->gety();
        $r2=$this->circleB->getRadius();

        $x3=$this->circleC->getx();
        $y3=$this->circleC->gety();
        $r3=$this->circleC->getRadius();

        $rx1= ($r2*$r2-$r1*$r1)-($x2*$x2-$x1*$x1)-($y2*$y2-$y1*$y1);
        $rx2= ($r3*$r3-$r1*$r1)-($x3*$x3-$x1*$x1)-($y3*$y3-$y1*$y1);

        $px= ($rx2*($y1-$y2)-$rx1*($y1-$y3))/(2*(($x1-$x3)*($y1-$y2)-($x1-$x2)*($y1-$y3)));
        $py= ($rx2*($x1-$x2)-$rx1*($x1-$x3))/(2*(($y1-$y3)*($x1-$x2)-($y1-$y2)*($x1-$x3)));

        $triPtX = sprintf("%.2f",$px);
        $triPtY = sprintf("%.2f",$py);
        $triPtZ = sprintf("%.2f",0);

        return compact('triPtX', 'triPtY','triPtZ');

    }

    /**
     *  其中两个相交则曲其中靠近第三个原的一个点
     * getPoint2CircleCross
     * @return array
     * Created by: xiebaoxin
     */
    public function getPoint2CircleCross($c1,$c2,$c3)
    {
        $circle1="circle".$c1;
        $circle2="circle".$c2;
        $circle3="circle".$c3;

        $x1=$this->$circle1->getx();
        $y1=$this->$circle1->gety();
        $r1=$this->$circle1->getRadius();

        $x2=$this->$circle2->getx();
        $y2=$this->$circle2->gety();
        $r2=$this->$circle2->getRadius();

        $x3=$this->$circle3->getx();
        $y3=$this->$circle3->gety();
        $r3=$this->$circle3->getRadius();


        //x的两个根 x_1 , x_2
        //y的两个根 y_1 , y_2
        $x_1=0;
        $x_2=0;
        $y_1=0;
        $y_2=0;

        //判别式的值
        $delta = -1;

        //如果 y1!=y2
        if($y1!=$y2){

            //为了方便代入
            $A = ($x1*$x1 - $x2*$x2 +$y1*$y1 - $y2*$y2 + $r2*$r2 - $r1*$r1)/(2*($y1-$y2));
            $B = ($x1-$x2)/($y1-$y2);

            $a = 1 + $B * $B;
            $b = -2 * ($x1 + ($A-$y1)*$B);
            $c = $x1*$x1 + ($A-$y1)*($A-$y1) - $r1*$r1;

            //下面使用判定式 判断是否有解
            $delta=$b*$b-4*$a*$c;

            if($delta >0)
            {

                $x_1=(-$b+sqrt($b*$b-4*$a*$c))/(2*$a);
                $x_2=(-$b-sqrt($b*$b-4*$a*$c))/(2*$a);
                $y_1 = $A - $B*$x_1;
                $y_2 = $A - $B*$x_2;
            }
            else if($delta ==0)
            {
                $x_1 = $x_2 = -$b/(2*$a);
                $y_1 = $y_2 = $A - $B*$x_1;
            }else
            {
             return null;
            }
        }
        else if($x1!=$x2){

            //当y1=y2时，x的两个解相等
            $x_1 = $x_2 = ($x1*$x1 - $x2*$x2 + $r2*$r2 - $r1*$r1)/(2*($x1-$x2));

            $a = 1 ;
            $b = -2*$y1;
            $c = $y1*$y1 - $r1*$r1 + ($x_1-$x1)*($x_1-$x1);

            $delta=$b*$b-4*$a*$c;

            if($delta >0)
            {
                $y_1 = (-$b+sqrt($b*$b-4*$a*$c))/(2*$a);
                $y_2 = (-$b-sqrt($b*$b-4*$a*$c))/(2*$a);
            }
            else if($delta ==0)
            {
                $y_1=$y_2=-$b/(2*$a);
            }else
            {
                return null;
            }
        }

       // return [$x_1,$y_1,$x_2,$y_2];
        $rx1=($x3-$x_1)*($x3-$x_1)+($y3-$y_1)*($y3-$y_1);
        $rx2= ($x3-$x_2)*($x3-$x_2)+($y3-$y_2)*($y3-$y_2);

        $triPtX = sprintf("%.2f",$rx1<$rx2? $x_1:$x_2);
        $triPtY = sprintf("%.2f",$rx1<$rx2? $y_1:$y_2);
        $triPtZ = sprintf("%.2f",0);

        return compact('triPtX', 'triPtY','triPtZ');

    }



    public function getIntersectionPoint()
    {

        if (!$this->circleA || !$this->circleB || !$this->circleC) {
            return NULL;
        }

        $vectorP1 = $this->circleA->getVector();
        $vectorP2 = $this->circleB->getVector();
        $vectorP3 = $this->circleC->getVector();

        #from wikipedia: http://en.wikipedia.org/wiki/Trilateration
        #transform to get circle 1 at origin
        #transform to get circle 2 on x axis

        // CALC EX
        $l = $vectorP2->subtract($vectorP1);

        $norm = new Vector(array_fill(0, 3, $l->length()));
        if(empty($norm)) return NULL;
        $d = $norm;
        $ex = $l->divide($norm);

        // CALC i
        $P3minusP1 = $vectorP3->subtract($vectorP1);
        $i = $ex->dotProduct($P3minusP1);

        // CALC EY
        $iex = $ex->multiplyByScalar($i);
        $P3P1iex = $P3minusP1->subtract($iex);
        $l = $P3P1iex;
        $norm = new Vector(array_fill(0, 3, $l->length()));
        if(empty($norm)) return NULL;
        $ey = $P3P1iex->divide($norm);

        // CALC EZ
        $ez = $ex->crossProduct($ey);

        // CALC D
        $d = $d->getElement(0) ;

        // CALC J
        $j = $ey->dotProduct($P3minusP1);

        #from wikipedia
        #plug and chug using above values
        $x = (pow($this->circleA->getRadius(), 2) - pow($this->circleB->getRadius(), 2) + pow($d, 2))/(2*$d);
        $y = ((pow($this->circleA->getRadius(), 2) - pow($this->circleC->getRadius(), 2) + pow($i, 2) + pow($j, 2))/(2*$j)) - (($i/$j)*$x);

        # only one case shown here
        $z = sqrt( pow($this->circleA->getRadius(),2) - pow($x,2) - pow($y,2) );

        #triPt is an array with ECEF x,y,z of trilateration point
        $xex = $ex->multiplyByScalar($x);
        $yey = $ey->multiplyByScalar($y);
        $zez = $ez->multiplyByScalar($z);

        // CALC $triPt = $P1vector + $xex + $yey + $zez;
        $triPt = $vectorP1
            ->add($xex)
            ->add($yey)
            ->add($zez);

        $triPtX = sprintf("%.2f",$triPt->getElement(0));
        $triPtY = sprintf("%.2f",$triPt->getElement(1));
        $triPtZ = sprintf("%.2f",$triPt->getElement(2));

       // echo "+0+++=".$triPtX."=".$triPtY."=".$triPtZ;

        return compact('triPtX', 'triPtY','triPtZ');

    }
}